COMMENT ⊗   VALID 00005 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	TITLE TLKEF5
C00004 00003	START OF EXECUTABLE CODE
C00007 00004		[CONTINUATION OF "TLKEF5"]
C00010 00005		[LOCAL STORAGE AREA]
C00013 ENDMK
C⊗;
TITLE TLKEF5
INTERNAL TLKEF5

;"TLKEF5" IS A SAIL CALLABLE PROCEDURE FOR READING THE CURRENT BLUE ARM
;POSITION FROM A SUPPORTING PROGRAM THAT RUNS ON THE PDP11/45.  A SAMPLE
;SAIL CALL IS AS FOLLOWS:
;
;	ERROR←TLKEF5(REAL ARRAY TRANS, ANGLES);
;WHERE
;	ERROR = 0 IF "TLKEF5" WAS SUCCESSFUL, ≠0 OTHERWISE
;	TRANS = 4x3 ARRAY IN WHICH THE CURRENT ARM TRANSFORM IS STORED
;		BY COLUMNS.
;	ANGLES= 1x7 ARRAY IN WHICH CURRENT JOINT ANGLES ARE RETURNED
;
;THIS ROUTINE TYPES IT'S OWN ERROR MESSAGES ON THE TTY.
;
;THIS VERSION HAS BEEN MODIFIED BY MSM TO REFLECT THE HANDLING OF SHORT TRANSFORMS.
;
;DEFINITIONS

P←17		;PUSH STACK REGISTER
DATADD←44040	;START ADDRESS TO READ DATA FROM ELF
MASTER←12121	;CHECK NUMBER FROM ELF IF READY
DBUFL ←=39	;NUMBER OF WORDS TO TRANSFER FROM THE ELF
;START OF EXECUTABLE CODE

TLKEF5:	SETZ	1,		;CLEAR ERROR FLAG
       	MOVEM	16,HOLD+16	;SAVE THE REGISTERS
	HRRZI	16,HOLD
	BLT	16,HOLD+15

;INTIALIZE THE ELF AND CLEAR THE MASTER NUMBER.  THIS INSURES THAT
;WE READ CURRENT DATA

	IOPUSH	1,		;CALLER MIGHT WANT THIS CHANNEL
	JRST	[   MOVEI   1,=6	;IOPDLOV MESSAGE
		    JRST   ERR  ]
	INIT	1,417  		;INITIALIZE THE ELF
	SIXBIT	/ELF/
	0
	JRST	[   MOVEI   1,=1		;ERROR RETURN 
ERR:		    MOVEM  1,HOLD+1	;RETURN ERROR VALUE
		    JRST   TLKDNE  ]
	GETSTS	1,1		;GET THE ELF STATUS WORD
	TRNE	1,777300	;CHECK FOR ERROR CONDITION
	JRST	[   MOVEI   1,=2		;INDICATE STATUS ERROR
		    JRST   ERR    ]
	MTAPE	1,MASADR	;ZERO MASTER NUMBER
	JRST	[   MOVEI   1,=3   	;ERROR RETURN
		    JRST   ERR  ]

;TRY TO COLLECT THE POSITION DATA
	
	MOVEI	3,=60		;TRY READING ELF DATA 60 TIMES
	MOVE	2,[IOWD DBUFL,ELFNMS]	;GET IO TRANSFER WORD
	MOVEI	4,MASTER	;GET THE ELF CHECK WORD
	JRST	.+3
READLP:	SETZ	1,		;SLEEP BEFORE TRYING AGAIN
	SLEEP	1,
       	USETI	1,DATA  	;SET UP INPUT OF DATA 
	MOVEM	2,INLST		;RESTORE IO TRANSFER WORD
     	IN	1,INLST		;READ IN A BLOCK OF DATA
	JRST	.+2		;NORMAL RETURN
	JRST	[   MOVEI   1,=5		;ERROR RETURN
		    JRST   ERR  ]
	CAMN 	4,ELFNMS	;SKIP IF NOT RUNNING
	JRST	GOTDAT
	SOJG	3,READLP	;REPEAT IF MORE TIME LEFT
	MOVEI	1,=4 		;ELSE TELL OPERATOR ELF TOOK TOO MUCH TIME
	JRST	ERR
;	[CONTINUATION OF "TLKEF5"]

;CONVERT THE ARRAY ELEMENTS FROM PDP11 FLOATING POINT TO PDP10 FORMAT

GOTDAT:	MOVE	5,-2(P)		;GET THE ADDRESS OF THE TRANS ARRAY
	MOVEI	4,TRANS		;GET ADDRESS OF TRANS DATA FROM ELF
	MOVEI	6,=12		;SEND BACK THE 12 ELEMENTS
	PUSHJ	P,CONV
	MOVE	5,-1(P)		;GET THE ADDRESS OF THE JOINT ARRAY
	MOVEI	4,JOINTS	;GET ADDRESS OF JOINT DATA FROM ELF
	MOVEI	6,7		;SEND BACK 7 JOINT ANGLES IN ALL
	PUSHJ	P,CONV

;RETURN TO CALLING PROGRAM
	SETZM	HOLD+1
TLKDNE:	IOPOP	1,		;RELEASE THE ELF & RESTORE CHANNEL 1
	JFCL			;JUST CANNOT HAPPEN
       	HRLZI	16,HOLD		;RESTORE THE REGISTERS
	BLT	16,16
	SUB	P,[3(3)]	;POP ARGUMENTS OFF STACK
	JRST	@3(P)		;RETURN


;SUBROUTINE FOR CONVERTING FROM PDP11/45 FLOATING POINT FORMAT TO THAT
;OF THE PDP10

CONV:   MOVE 	1,(4)		;GET 16 MSB
        LSH    	1,20
        MOVE   	2,1(4)		;GET 16 LSB
        ANDI   	2,177777	;OR THEM TOGETHER
        IOR    	1,2
        SETZ   	2,
        DPB    	1,LBYTE		;MAKE ROOM FOR THE MSB OF THE MAGNITUDE
        LSH    	1,-27
        LSHC   	1,-11
        JUMPE  	2,DNE		
        TLO    	2,400		;TAKE CARE OF THE SIGN AND MSB MAGNI. BITS
        JUMPGE 	2,DNE
        TLZ    	2,400000
        MOVN   	2,2
DNE:  	MOVEM  	2,(5)		;RETURN THE PDP10 F.P. NUMBER
	ADDI	4,2		;UPDATE THE POINTERS
	AOJ	5,
	SOJG	6,CONV		;REPEAT FOR ALL REQUESTED NUMBERS
	POPJ	P,
;	[LOCAL STORAGE AREA]

HOLD:	BLOCK	17
ELFNMS:	0		;MASTER NUMBER RETURNED IN HERE
TRANS:	BLOCK	=24	;TRANSFORM 
JOINTS:	BLOCK	=14	;JOINT ANGLES
LBYTE:  142700000002	;BYTE POINTER FOR COMBINING TWO PDP11 WORDS

;I/O DATA AREAS

DATA:	400004,,400000+DATADD	;MODE AND ADDR TO READ DATA FROM THE ELF

INLST:	IOWD DBUFL,ELFNMS	;READ BACK ARM POSITION
	0

MASADR:	003000,,DATADD		;CLEAR MASTER WORD
	0

;OUTPUT STRINGS
; ALL THESE MESSAGES ARE NOW IN THE 10.  ERROR CODES CORREPSOND TO THEM
;CM0:	ASCIZ/IOPDLOV WHEN TRY TO SAVE CHANNEL 1 FOR "ELF"
;/

;CM1:	ASCIZ/CANNOT INIT "ELF"
;/
;CM2:	ASCIZ/"ELF" STATUS WORD INDICATES ERROR CONDITION
;/
;CM3:	ASCIZ/UNABLE TO ZERO MASTER NUMBER IN "ELF"
;/
;CM4:	ASCIZ/"ELF" NOT TRANSFERING ARM POSITION
;/
;CM5: 	ASCIZ/BAD READ FROM "ELF" DURING POSITION DATA TRANSFER
;/

END